#include <reactive_behavior/reactive_behavior.h>
#include <boost/thread/thread.hpp>
#include <cstdlib>
#include <algorithm>
#include <cmath>

#include <iostream> //TODO remove me!
#include <iomanip>

using std::cout;
using std::endl;

using std::setw;
using std::setprecision;

using std::min;
using std::max;

using std::rand;

// Constants.
//

static const double kPi = 3.141592653589793238462643;

enum ReactiveState {
  kDone,
  kLWallFollow,
  kRWallFollow,
  kGoalSeek,
};

const int ReactiveBehavior::kTimeout;
const double ReactiveBehavior::kAtGoalThreshold;
const double ReactiveBehavior::kObstacleThreshold;
const double ReactiveBehavior::kWallFollowThreshold;
const double ReactiveBehavior::kMaxForwardSpeed;
const double ReactiveBehavior::kGoalSeekRotScaler;
const double ReactiveBehavior::kMaxRotSpeed;

// Helper functions.
//

// Euclidean distance between two points, squared.
inline double GetDistance(player_point_2d a, player_point_2d b) {
  double dx = a.px - b.px;
  double dy = a.py - b.py;
  return sqrt(dx * dx + dy * dy);
}

// Clamps values to given range.
inline double ClampToRange(double value, double start, double end) {
  if (value < start) return start;
  else if(value > end) return end;
  return value;
}

void IndexesForAngles(double start_angle, double end_angle, int *left, int *right, bool debug = false) {
  /////cout << (*sonar_proxy) << endl;  // TODO TEST
  start_angle = rtod(start_angle);  // Convert from radians to degrees.
  end_angle = rtod(end_angle);
  if (start_angle < -70.0) *right = 7;
  else *right = (70.0 - start_angle) / 20;
  if (end_angle > 70.0 ) *left = 0;
  else *left = ceil((70.0 - end_angle) / 20);
  if (debug) {
    ///cout << "sonars facing the goal: " << "from: " << (*left) << " to: " << (*right) << endl;  // TEST
  }
}

// Minimum sonar range (among all available sonar rangers).
double MinRange(SonarProxy *sonar_proxy, double start_angle, double end_angle, bool debug = false){
  int left, right;
  IndexesForAngles(start_angle, end_angle, &left, &right, debug);
  // if (debug)
    // cout << "sonars facing goal: " << left << " " << right << endl;
  double min_range = (*sonar_proxy)[left];
  for (int i = left + 1; i <= right; i++) {
    double range = (*sonar_proxy)[i];
    if (range < min_range) min_range = range;
  }
  return min_range;
}

// Minimum sonar range (among all available sonar rangers).
double WeightedMinRange(SonarProxy *sonar_proxy, double start_angle, double end_angle, bool debug = false){
  double weight[8] = {5, 1.5, 1.2, 1, 1, 1.2, 1.5, 5};
  int left, right;
  IndexesForAngles(start_angle, end_angle, &left, &right, debug);
  double min_range = weight[left] * (*sonar_proxy)[left];
  //cout << "range " << left << " " << min_range << endl;
  for (int i = left + 1; i <= right; i++) {
    double range = weight[i] * (*sonar_proxy)[i];
    //cout << "range " << i << " " << range << endl;
    if (range < min_range) min_range = range;
  }
  // cout << endl;
  return min_range;
}

// Average sonar range (among all available sonar rangers).
double AvgRange(SonarProxy *sonar_proxy, double start_angle, double end_angle, bool debug = false){
  int left, right;
  IndexesForAngles(start_angle, end_angle, &left, &right, debug);
  double avg_range = (*sonar_proxy)[left];
  for (int i = left + 1; i <= right; i++) {
    avg_range += (*sonar_proxy)[i];
  }
  avg_range = avg_range / (right - left + 1);
  return avg_range;
}

// Geometric mean sonar range (among all available sonar rangers).
double GeomAvgRange(SonarProxy *sonar_proxy, double start_angle, double end_angle, bool debug = false){
  int left, right;
  IndexesForAngles(start_angle, end_angle, &left, &right, debug);
  double avg_range = (*sonar_proxy)[left];
  for (int i = left + 1; i <= right; i++) {
    avg_range *= (*sonar_proxy)[i];
  }
  avg_range = pow(avg_range, 1.0 / (right - left + 1));
  return avg_range;
}

// Member function implementations.
//

ReactiveBehavior::ReactiveBehavior(PlayerClient *player_client,
                                   Position2dProxy *position2d_proxy,
                                   SonarProxy *sonar_proxy,
                                   SyncedCurrentPose *synced_current_pose):
    player_client_(player_client),
    position2d_proxy_(position2d_proxy),
    sonar_proxy_(sonar_proxy),
    synced_current_pose_(synced_current_pose) {}

double ReactiveBehavior::ComputeForwardSpeed() {
  double min_sonar_front = WeightedMinRange(sonar_proxy_, -kPi / 2, kPi / 2);
  ///cout << "min sonar front: " << min_sonar_front << endl;
  if (min_sonar_front < kObstacleThreshold) {
    ///cout << "STOOOOOOOOOOOOOOOOOOP" << endl;
    return 0.0;  // Very close obstacle.
  } else {
    return min(kMaxForwardSpeed, min_sonar_front * kForwardSpeedScaler);  // All is fine, move.
  }
}

bool ReactiveBehavior::ObstaclesInWay(const double &goal_angle) {
  double goal_angle_begin = goal_angle - kPi / 3;
  double goal_angle_end = goal_angle + kPi / 3;
  //goal_angle_begin = ClampToRange(goal_angle_begin, dtor(-70.0), dtor(70.0));
  //goal_angle_end = ClampToRange(goal_angle_end, dtor(-70.0), dtor(70.0));
  ///cout << "sonar ring angle facing goal: " << rtod(goal_angle_begin) << " " << rtod(goal_angle_end) << endl; 
  double min_sonar_value;
  if ((goal_angle_begin < dtor(-70.0) || goal_angle_begin > dtor(70.0)) && 
      (goal_angle_end < dtor(-70.0) || goal_angle_end > dtor(70.0))) {
    // Trying to treat the very bad condition where none of the
    // robot's sonars is facing the goal and therefore, its impossible
    // to tell whether or not the straight line path to the goal is clear.
    // Assume the path to the goal is either going left or right.
    double min_left, min_right;
    min_left = MinRange(sonar_proxy_, 0.0, kPi / 2);  // Left side readings.
    min_right = MinRange(sonar_proxy_, -kPi / 2, 0.0); // Right side readings.
    min_sonar_value = min(min_left, min_right);
  } else { 
    // Goal is visible and all is fine.
    min_sonar_value = MinRange(sonar_proxy_, 
                               goal_angle_begin,
                               goal_angle_end, true);
    // Detects obstacle when frontal range is 12% longer
    // than the one that forces full translational stop.
  }
  // Obs: 1.1 factor so obstacle gets detected just before full forward stop.
  return (min_sonar_value < 1.2 * kObstacleThreshold);
}

double ReactiveBehavior::ComputeGoalSeekRot(const double &goal_angle) {
  /////cout << "given goal_angle: " << fabs(goal_angle) << endl;  // TEST
  /////cout << "too small angle: " << (kPi / 10) << endl;  // TEST
  if (fabs(goal_angle) < kPi / 35) return 0.0;  // Goal angle too small? Return zero.
  double desired_turn_speed = goal_angle * kGoalSeekRotScaler;
  desired_turn_speed = ClampToRange(desired_turn_speed, -kMaxRotSpeed, kMaxRotSpeed);
  if (fabs(desired_turn_speed) < 0.13) 
    desired_turn_speed = desired_turn_speed < 0 ? -0.25 : 0.25;
  ///cout << "GoalSeekRot speed (deg/s): " << rtod(turn_speed) << endl;
  return desired_turn_speed;
}

double ReactiveBehavior::ComputeLWFRot() {
  double min_left, desired_turn_speed;
  min_left = MinRange(sonar_proxy_, 0.0, kPi / 2);  // Left side readings.
  // if (max(min_right, min_left) < kObstacleThreshold) return kMaxRotSpeed;  // TEST
  // Calculate desired turn and clamp it.
  desired_turn_speed = (min_left - kWallFollowThreshold) * kWallFollowRotScaler;  // Turns faster, if closer to obstacles.
  desired_turn_speed = ClampToRange(desired_turn_speed, -kMaxRotSpeed, kMaxRotSpeed);
  if (fabs(desired_turn_speed) < 0.13) 
    desired_turn_speed = desired_turn_speed < 0 ? -0.25 : 0.25;
  ///cout << "LWFRot speed (deg/s): " << rtod(desired_turn_speed) << endl;
  return desired_turn_speed;
}

double ReactiveBehavior::ComputeRWFRot() {
  double min_right, desired_turn_speed;
  min_right = MinRange(sonar_proxy_, -kPi / 2, 0.0); // Right side readings.
  // if (max(min_right, min_left) < kObstacleThreshold) return kMaxRotSpeed;  // TEST
  // Calculate desired turn and clamp it.
  desired_turn_speed = (kWallFollowThreshold - min_right) * kWallFollowRotScaler;  // Turns faster, if closer to obstacles.
  desired_turn_speed = ClampToRange(desired_turn_speed, -kMaxRotSpeed, kMaxRotSpeed);
  if (fabs(desired_turn_speed) < 0.13) 
    desired_turn_speed = desired_turn_speed < 0 ? -0.25 : 0.25;
  ///cout << "min_right: " << min_right << endl;
  ///cout << "RWFRot speed (deg/s): " << rtod(desired_turn_speed) << endl;
  return desired_turn_speed;
}

void ReactiveBehavior::RotateRandomly() {
  player_point_2d position = synced_current_pose_->current_position();
  position2d_proxy_->GoTo(position.px, position.py, 2 * kPi * rand() / RAND_MAX);
}

bool ReactiveBehavior::IsRobotStuck(player_point_2d position) {
  const int N = 70;
  static player_point_2d positions[N];
  static int i = 0;
  if (i < N) {
    positions[i++] = position;
  } else {
    i = 0;
    player_point_2d mean = {0.0, 0.0};
    for (int k = 0; k < N; k++) {
      mean.px += positions[k].px;
      mean.py += positions[k].py;
    }
    mean.px = mean.px / N;
    mean.py = mean.py / N;
    player_point_2d std_dev = {0.0, 0.0};
    for (int k = 0; k < N; k++) {
      double diff_x =  positions[k].px - mean.px;
      double diff_y =  positions[k].py - mean.py;
      std_dev.px += diff_x * diff_x;
      std_dev.py += diff_y * diff_y;
    }
    std_dev.px = sqrt(std_dev.px / N);
    std_dev.py = sqrt(std_dev.py / N);
    if (fabs(std_dev.px) < 0.7 && fabs(std_dev.py) < 0.7) {
      ///cout << ">>>>>>>>> Warning! ROBOT MIGHT BE STUCK, std dev: " << std_dev.px << " " << std_dev.py << endl;  // TEST
      return true;
    }
  }
  return false;
}

// robot_angle must belong to [-pi, pi]
double AngleToGoal(double phi, double robot_angle) {
  double theta = robot_angle;
  double lambda = phi - theta;
  if (lambda < -kPi) {
    return lambda + 2 * kPi;
  } else if (kPi < lambda) {
    return lambda - 2 * kPi;
  } else {
    return lambda;
  }
}

// robot_angle must belong to [-pi, pi]
double AngleToGoal(player_point_2d robot_position, player_point_2d goal_position, double robot_angle) {
  double phi = atan2(goal_position.py - robot_position.py, goal_position.px - robot_position.px);
  return AngleToGoal(phi, robot_angle);
}

void PrintSonarMeasurements(SonarProxy *sonar_proxy) {
  for (int i = 0; i <= 7; i++) {
    cout << setw(7) << setprecision(4) << (*sonar_proxy)[i] << " ";
  }
  cout << endl;
  for (int i = 0; i <= 7; i++) {
    cout << setw(7) << i << " ";
  }
  cout << endl;
}

bool ReactiveBehavior::MoveToGoalAvoidingObstacles(const player_point_2d &goal_pos) {
  time_t start_time, current_time;
  time_t start, end_block, end;
  double diff;
  double diff_block;
  ReactiveState state = kGoalSeek;  // Initialize state as Goal Seek.
  bool at_goal = false;
  time(&start_time);
  while (!at_goal) {
    time(&start);
    player_client_->Read();
    // cout << "################################################################" << endl;
    //cout << (*sonar_proxy_) << "\n" << endl;
    // PrintSonarMeasurements(sonar_proxy_);
    // cout << "################################################################//" << endl;
    player_pose2d robot_pose = synced_current_pose_->current_pose();
    // cout << "ESTOU AQUI: " << robot_pose.px << " " << robot_pose.py << " " << robot_pose.pa << endl;
    // cout << "quero ir para: " << goal_pos.px << " " << goal_pos.py << endl;
    time(&end_block);
    diff_block = difftime(end_block, start);
    ///cout << "blocking time: " << diff_block << endl;
    double robot_angle = robot_pose.pa;
    ///cout << "robot angle angle/yaw: " << rtod(robot_angle) << endl;  // TEST
    player_point_2d robot_pos;
    robot_pos.px = robot_pose.px; 
    robot_pos.py = robot_pose.py;
    /*if (IsRobotStuck(robot_pos)) {
      position2d_proxy_->SetSpeed(0.0, 0.0);
      RotateRandomly();
      sleep(3);
      continue;
    }*/ 
    double goal_dist = GetDistance(robot_pos, goal_pos);
    //cout << "goal pos: " << goal_pos.px << " " << goal_pos.py << endl;  // TEST
    ///cout << "robot pos: " << robot_pos.px << " " << robot_pos.py << endl;  // TEST
    // cout << "distance to goal: " << goal_dist << endl;  // TEST
    double goal_angle = AngleToGoal(robot_pos, goal_pos, robot_angle);
    // cout << "angle to goal (deg): " << rtod(goal_angle) << endl;  // TEST
    double forward_speed, rotation_speed;
    if (goal_dist < kAtGoalThreshold) {
      // cout << "<<<<<<<<<<<<<<<<<<<<<<<<<<<REACTIVE: GOAL REACHED!!!>>>>>>>>>>>>>>>>>>>>>>>>>>" << endl;
      forward_speed = rotation_speed = 0;
      state = kDone;
      at_goal = true;
    } else {
      forward_speed = ComputeForwardSpeed();
      if (state == kGoalSeek) {
        // cout << "GOAL SEEK MODE" << endl;  // TEST
        rotation_speed = ComputeGoalSeekRot(goal_angle);
        ///cout << "turning speed (deg/s): " << rtod(rotation_speed) << endl;  // TEST
        if (ObstaclesInWay(goal_angle)) {
          double min_left, min_right;
          min_left = AvgRange(sonar_proxy_, 0.0, kPi / 2);  // Left side readings.
          min_right = AvgRange(sonar_proxy_, -kPi / 2, 0.0); // Right side readings.
          if (min_left < min_right) {
            // Less space on the left, so do left wall follow 
            // (i.e. wall on the left). 
            state = kLWallFollow;
          } else {
            state = kRWallFollow;
          }
        }
      }
      if (state == kLWallFollow) {
        // cout << "LEFT WALL FOLLOW MODE" << endl;  // TEST
        rotation_speed = ComputeLWFRot();
        if (!ObstaclesInWay(goal_angle))
          state = kGoalSeek;
      } if (state == kRWallFollow) {
        // cout << "RIGHT WALL FOLLOW MODE" << endl;  // TEST
        rotation_speed = ComputeRWFRot();
        if (!ObstaclesInWay(goal_angle))
          state = kGoalSeek;
      }
    }
    //if (fabs(rotation_speed) * 7 >= 1) 
      // Forward speed reduction on rotations above 1 degree/sec.
    //  forward_speed = ComputeForwardSpeed() / fabs(rotation_speed);
    //else 
    // just compute forward speed
    if (fabs(rotation_speed) > .30)
      forward_speed = 0.0;
    else if (goal_dist < 0.70)
      forward_speed = 0.10;
    // cout << "forward speed (m/s): " << forward_speed << endl;
    // cout << "rotation speed (deg/s): " << rtod(rotation_speed) << endl;
    position2d_proxy_->SetSpeed(forward_speed, rotation_speed);
    boost::this_thread::sleep(boost::posix_time::milliseconds(50));  // Save CPU time.
    time(&end);
    diff = difftime(end, start);
    ///cout << "cycle period: " <<  diff << endl;
    ///cout << endl << endl;  // TEST
    // Timeout
    time(&current_time);
    if (difftime(current_time, start_time) > 90.0)
      return false;
  }
  return true;
}

void ReactiveBehavior::RotateTo(const double &phi) {
  double rotation_speed, goal_angle;
  do {
    goal_angle = AngleToGoal(phi, synced_current_pose_->current_pose().pa);
    rotation_speed = ComputeGoalSeekRot(goal_angle);
    position2d_proxy_->SetSpeed(0, rotation_speed);
    boost::this_thread::sleep(boost::posix_time::milliseconds(50));
  } while (rotation_speed > 0);
}
